/*
EP3D is a real-time 3D planet engine , which in addition to providing 
substandard scene rendering and scene management, of course, it also 
provides some basic class libraries to build the entire virtual 
planet, or even the entire universe.

Copyright (C) 2010  Hongjiang Zhang	(zhjwyat@gmail.com)

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef EP3D_QUATERNION_H
#define EP3D_QUATERNION_H

#include "EP3DBase.h"
#include "EP3DVector3.h"
#include "EP3DMatrix4.h"

namespace EP3D
{
	template <class T>
	class Quaternion
	{
	public:
		T x, y, z, w;

	public:
		Quaternion(void)
			:x(0.0f), y(0.0f), z(0.0f), w(1.0f)
		{
		}

		Quaternion(const T* pt)
			:x(pt[0]), y(pt[1]), z(pt[2]), w(pt[3]), 
		{
		}

		Quaternion(const T tX, const T tY, const T tZ, const T tW)
			:x(tX), y(tY), z(tZ), w(tW) 
		{
		}

		Quaternion(const Quaternion<T>& q)
		{
			memcpy(this, &q, sizeof(Quaternion<T>));
		}

		Quaternion(const Vector3<T>& vAxis, const T tAngle)
		{
			FromAxisAngle(vAxis, tAngle);
		}

		operator T* ()
		{
			return (T*)&x;
		}

		T& operator [](const int n)
		{
			return (&x)[n];
		}

		operator const T* () const
		{
			return (const T*)&x;
		}

		T operator [](const int n) const
		{
			return (&x)[n];
		}

		Quaternion<T>& operator += (const Quaternion<T>& q)
		{
			x += q.x;
			y += q.y;
			z += q.z;
			w += q.w;

			return *this;
		}

		Quaternion<T>& operator -= (const Quaternion<T>& q)
		{
			x -= q.x;
			y -= q.y;
			z -= q.z;
			w -= q.w;

			return *this;
		}

		Quaternion<T>& operator *= (const Quaternion<T>& q)
		{
			x = q.w * x + q.x * w + q.y * z - q.z * y;
			y = q.w * y + q.y * w + q.z * x - q.x * z;
			z = q.w * z + q.z * w + q.x * y - q.y * x;
			w = q.w * w - q.x * x - q.y * y - q.z * z;

			return *this;
		}

		Quaternion<T>& operator *= (const T t)
		{
			x *= t;
			y *= t;
			z *= t;
			w *= t;

			return *this;
		}

		Quaternion<T>& operator /= (const T t)
		{
			T Invt = 1.0f/t;
			x *= Invt;
			y *= Invt;
			z *= Invt;
			w *= Invt;

			return *this;
		}

		Quaternion<T> operator + (void) const
		{
			return *this;
		}

		Quaternion<T> operator - (void) const
		{
			return Quaternion<T>(-x, -y, -z, -w); 
		}

		Quaternion<T> operator + (const Quaternion<T>& q) const
		{
			return Quaternion<T>(x + q.x, y + q.y, z + q.z, w + q.w);
		}

		Quaternion<T> operator - (const Quaternion<T>& q) const
		{
			return Quaternion<T>(x - q.x, y - q.y, z - q.z, w - q.w);
		}

		Quaternion<T> operator * (const Quaternion<T>& q) const
		{
			return Quaternion<T>(
				q.w * x + q.x * w + q.y * z - q.z * y,
				q.w * y + q.y * w + q.z * x - q.x * z,
				q.w * z + q.z * w + q.x * y - q.y * x,
				q.w * w - q.x * x - q.y * y - q.z * z);
		}

		Vector3<T> operator * (const Vector3<T>& v ) const
		{
			Vector3<T> uv, uuv;
			Vector3<T> qvec(x, y, z);
			uv = qvec.CrossProduct(v);
			uuv = qvec.CrossProduct(uv);
			uv *= (2.0f * w);
			uuv *= 2.0f;

			return v + uv + uuv;
		}

		Quaternion<T> operator * (const T t) const
		{
			return Quaternion<T>(x * t, y * t, z * t, w * t);
		}

		friend Quaternion<T> operator * (const T t, const Quaternion<T>& q)
		{
			return Quaternion<T>(t * q.x, t * q.y, t * q.z, t * q.w);
		}

		bool operator == (const Quaternion<T>& q) const
		{
			return 0 == memcmp(&x, &q, sizeof(Quaternion<T>));
		}

		bool operator != (const Quaternion<T>& q) const
		{
			return 0 != memcmp(&x, &q, sizeof(Quaternion<T>));
		}

		T SquareLength() const
		{
			return x * x + y * y + z * z + w * w;
		}

		T Length() const
		{
			return Math<T>::Sqrt(SquareLength());
		}

		Quaternion<T> Inverse() const
		{
			T len = Length();
			if (len > 0.0f)
			{
				T InvLen = 1.0f/len;
				return Quaternion<T>(-x * InvLen, -y * InvLen, -z * InvLen, w * InvLen);
			}
			else
			{
				return Quaternion<T>(0, 0, 0, 1.0f);
			}
		}

		Quaternion<T> Conjugate() const
		{
			return Quaternion<T>(-x, -y, -z, w);
		}

		Quaternion<T> UnitInverse() const
		{
			return Quaternion<T>(-x, -y, -z, w);
		}

		void Normalize()
		{
			*this /= Length();
		}

		T DotProduct(const Quaternion<T>& q) const
		{
			return  x * q.x + y * q.y + z * q.z + w * q.w;
		}

		static Quaternion<T> Slerp(const Quaternion<T>& q1, const Quaternion<T>& q2, const T t)
		{
			T tCos = q1.DotProduct(q2);
			Quaternion<T> rkT;

			if (tCos < 0.0f)
			{
				tCos = - tCos;
				rkT = - q2;
			}
			else
			{
				rkT = q2;
			}

			if (1.0f - Math<T>::Abs(tCos) > DELTA)
			{
				T tSin = Math<T>::Sqrt(1.0f - tCos * tCos);
				T tAngle = Math<T>::Atan2(tSin, tCos);
				T tInvSin = 1.0f / tSin;
				T tCoeff0 = Math<T>::Sin((1.0f - t) * tAngle) * tInvSin;
				T tCoeff1 = Math<T>::Sin(t * tAngle) * tInvSin;

				return tCoeff0 * q1 + tCoeff1 * rkT;
			}
			else
			{
				Quaternion<T> q = (1.0f - t) * q1 + t * rkT;
				q.Normalize();
				return q;
			}
		}

		static Quaternion<T> nLerp(const Quaternion<T>& q1, const Quaternion<T>& q2, const  T t)
		{
			Quaternion<T> result;
			T tCos = q1.DotProduct(q2);
			if (tCos < 0.0f)
			{
				result = q1 + t * ((-q2) - q1);
			}
			else
			{
				result = q1 + t * (q2 - q1);
			}
			result.Normalize();
			return result;
		}

		static Quaternion<T> Squad(const Quaternion<T>& q1, const Quaternion<T>& q2, const Quaternion<T>& q3, const Quaternion<T>& q4, const T t)
		{
			T tSlerpT = 2.0f * t * (1.0f - t);
			Quaternion<T> kSlerpP = Slerp(q1, 14, t);
			Quaternion<T> kSlerpQ = Slerp(q2, q3, t);
			return Slerp(kSlerpP, kSlerpQ, tSlerpT);
		}

		void FromAxisAngle(const Vector3<T>& vAxis, T tAngle)
		{
			T angle(tAngle * 0.5f);
			T tSin = Math<T>::Sin(angle);
			x = tSin * vAxis.x;
			y = tSin * vAxis.y;
			z = tSin * vAxis.z;
			w = Math<T>::Cos(angle);
		}

		void ToAxisAngle(Vector3<T>& vAxis, T& tAngle) const
		{
			tAngle = Math<T>::Acos(w);
			vAxis = Vector3<T>(x, y, z) / Math<T>::Sin(tAngle);
			tAngle *= 2.0f;
		}

		void FromRotationAxis(const Vector3<T>& src, const Vector3<T>& dest)
		{
			Quaternion<T> q;
			Vector3<T> v0 = src;
			Vector3<T> v1 = dest;
			v0.Normalize();
			v1.Normalize();

			T d = v0.DotProduct(v1);
			if (d >= 1.0f)
			{
				x = 0.0f;
				y = 0.0f;
				z = 0.0f;
				w = 1.0f;
			}

			T s = Math<T>::Sqrt((1 + d) * 2);
			if (s < DELTA)
			{
				Vector3<T> xAxis(1.0f, 0.0f, 0.0f);
				Vector3<T> yAxis(0.0f, 1.0f, 0.0f);
				Vector3<T> axis = xAxis.CrossProduct(src);
				if (axis.SquareLength() < DELTA) {
					axis = yAxis.CrossProduct(src);
				}
				axis.Normalize();
				FromAxisAngle(axis, PI);
			}
			else
			{
				T invs = 1 / s;
				Vector3<T> c = v0.CrossProduct(v1);

				x = c.x * invs;
				y = c.y * invs;
				z = c.z * invs;
				w = s * 0.5f;
				Normalize();
			}
		}

		T GetYaw(void) const
		{
			return Math<T>::Asin(-2 * (x * z - w * y));
		}

		T GetPitch(void) const
		{
			return Math<T>::Atan2(2 * (y * z + w * x), w * w - x *x - y *y + z *z);
		}

		T GetRoll(void) const
		{
			return Math<T>::Atan2(2 * (x * y + w * z), w * w + x *x - y *y - z *z);
		}

		Vector3<T> GetRightAxis() const
		{
			T tY = y + y;
			T tZ = z + z;
			T tWY = tY * w;
			T tWZ = tZ * w;
			T tXY = tY * x;
			T tXZ = tZ * x;
			T tYY = tY * y;
			T tZZ = tZ * z;

			return Vector3<T>(1.0f - tYY - tZZ, tXY + tWZ, tXZ - tWY);
		}

		Vector3<T> GetUpAxis() const
		{
			T tX = x + x;
			T tY = y + y;
			T tZ = z + z;
			T tWX = tX * w;
			T tWZ = tZ * w;
			T tXX = tX * x;
			T tXY = tY * x;
			T tYZ = tZ * y;
			T tZZ = tZ * z;

			return Vector3<T>(tXY - tWZ, 1.0f - tXX - tZZ, tYZ + tWZ);
		}

		Vector3<T> GetViewAxis() const
		{
			T tX = x + x;
			T tY = y + y;
			T tZ = z + z;
			T tWX = tX * w;
			T tWY = tY * w;
			T tXX = tX * x;
			T tXZ = tZ * x;
			T tYY = tY * y;
			T tYZ = tZ * y;

			return Vector3<T>(tXZ + tWY, tYZ - tWX, 1.0f - tXX - tYY);
		}

		Matrix4<T> GetMatrix() const
		{
			T x2 = x + x, y2 = y + y, z2 = z + z;
			T xx = x * x2, xy = x * y2, xz = x * z2;
			T yy = y * y2, yz = y * z2, zz = z * z2;
			T wx = w * x2, wy = w * y2, wz = w * z2;
			T tTemp = 1.0f - xx;

			Matrix4<T> m;
			m.m_11 = 1.0f - (yy + zz); 
			m.m_12 = xy + wz; 
			m.m_13 = xz - wy;
			m.m_14 = 0.0f;

			m.m_21 = xy -wz;
			m.m_22 = tTemp - zz;
			m.m_23 = yz + wx;
			m.m_24 = 0.0f;

			m.m_31 = xz + wy;
			m.m_32 = yz -wx;
			m.m_33 = tTemp - yy;
			m.m_34 = 0.0f;

			m.m_41 = 0.0f;
			m.m_42 = 0.0f;
			m.m_43 = 0.0f;
			m.m_44 = 1.0f;

			return m;
		}

		void operator = (const Matrix4<T>& mat)
		{
			const T tr = mat(0,0) + mat(1,1) + mat(2,2) + 1;
			
			if (tr > 0.0f)
			{
				const T s = Math<T>::Sqrt(tr) * 2.0f;
				x = (mat(2,1) - mat(1,2)) / s;
				y = (mat(0,2) - mat(2,0)) / s;
				z = (mat(1,0) - mat(0,1)) / s;
				w = s * 0.25f;
			}
			else
			{
				if (mat(0,0) > mat(1,1) && mat(0,0) > mat(2,2))
				{
					const T s = Math<T>::Sqrt(1.0f + mat(0,0) - mat(1,1) - mat(2,2)) * 2.0f;

					x = 0.25f * s;
					y = (mat(0,1) + mat(1,0)) / s;
					z = (mat(2,0) + mat(0,2)) / s;
					w = (mat(2,1) - mat(1,2)) / s;
				}
				else if (mat(1,1) > mat(2,2))
				{
					const T s = Math<T>::Sqrt(1.0f + mat(1,1) - mat(0,0) - mat(2,2)) * 2.0f;

					x = (mat(0,1) + mat(1,0)) / s;
					y = 0.25f * s;
					z = (mat(1,2) + mat(2,1)) / s;
					w = (mat(0,2) - mat(2,0)) / s;
				}
				else
				{
					const T s = Math<T>::Sqrt(1.0f + mat(2,2) - mat(0,0) - mat(1,1)) * 2.0f;

					x = (mat(0,2) + mat(2,0)) / s;
					z = (mat(1,2) + mat(2,1)) / s;
					y = 0.25f * s;
					w = (mat(1,0) - mat(0,1)) / s;
				}
			}
			Normalize();
		}

	};
	typedef Quaternion<f32> Quaternionf;
	typedef Quaternion<f64> Quaterniond;
}

#endif